#include "blob_detect.h"
#include "output.h"
#include "2-00/nxt.h"

#include <stdio.h>
#include <iostream>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"

using namespace cv;

//Method for loading video feed for single blob detection.
IplImage* GetThresholdedImage(IplImage* img, IplImage* result)
{
	// Convert the image into an HSV image
	IplImage* imgHSV = cvCreateImage(cvGetSize(img), 8, 3);
	cvCvtColor(img, imgHSV, CV_BGR2HSV);

	IplImage* imgThreshed = cvCreateImage(cvGetSize(img), 8, 1);

	// Values 20,100,100 to 30,255,255 working perfect for yellow at around 6pm
	cvInRangeS(imgHSV, cvScalar(20, 100, 100), cvScalar(30, 255, 255), imgThreshed); // yellow
	//cvInRangeS(imgHSV, cvScalar(85, 100, 100), cvScalar(130, 255, 255), imgThreshed);// blue

	cvReleaseImage(&imgHSV);

	return imgThreshed;
}

//Method for single blob detection.
void ObjectDetect(IplImage* img, IplImage* result, IplImage* frame){
	int Mass = 0;
	int Y = 0;
	int X = 0;
	CvScalar PixelValue;
	CvScalar Centre;
	//BGR
	//Centre.val[1]=255;
	Centre.val[0]=255;
	cvNamedWindow("result");

	result = frame;

	for(int Row = 0; Row < img->height; Row++){
		for(int Col = 0; Col < img->height; Col++){
			PixelValue = cvGet2D(img, Row, Col);

			if(PixelValue.val[0] == 255){
				Mass++;
				Y = Y + Row;
				X = X + Col;
			}
		};
	};
	if(Mass != 0 || Y != 0 || X != 0){
		Y = Y/Mass;
		X = X/Mass;
		cvSet2D(result, Y, X, Centre);
	}
	else{
		cout << "No object detected" << endl;
	};
};

int main()
{
	BlobDetector blob;
	//OutputModule output;
	blob.LastTransX = 0;
	blob.LastTransY = 0;
	VideoCapture Cam(1);
	Cam >> blob.Frame;
	blob.LoadLiveFeed(Cam);
    //output.SetupCom(blob.WorkImage.cols, blob.WorkImage.rows);

	while(true)
	{
		blob.LoadLiveFeed(Cam);
		blob.Image_Scan();
		cout << "LastMassCol: " << blob.LastMassCol << endl;

		cvWaitKey(1);
	}

	return 0;
}
